With the display and sensor functions discussed and worked through, the most interesting aspect of a mobile robot application can be explored - movement. In this book, I have discussed how a simple robot consisting of a modified toy or two radio control servos could be created. With a mobile base available along with a set of sensors and I/O devices to choose from, I came up with the following basic set of biologic code function prototypes that could be used to create the robot application:
int GetLeftWhisker(); // Return State of Left Collision Sensor
int GetRightWhisker(); // Return State of Right Collision Sensor
// The "Whiskers" can either be a physical sensor or IR
int GetLeftLight(); // Read the Left Light Sensor Value
int GetRightLight(); // Read the Right Light Sensor Value
// Light sensors can either be two CDS cells forming a voltage
// divider or a resistance measuring routine using the CDS cells
void LeftMotor(int Movement[, int Speed]); // Set Left Motor Op’n
void RightMotor(int Movement[, int Speed]); // Set Right Motor Op’n
// The "Speed" parameter is not available for RC Servos
void LED(int State); // Turn On/Off LED
void LCDOutput(char * Message); // Output a Message on an LCD
void Dlay(int msecs); // Delay the set number of msecs
Click on the example application names below to look at the source code:
The Tamiya "Wall Hugging Mouse" is an excellent base to get started with
robots on. The kit is reasonably inexpensive and provides a large plastic base which can
be used to mount control circuitry on. In the picture at the right, you can see how I
created a mobile robot based on the Tamiya kit.
\code\wallhug\wallhug.c is the base level of code for the robot and includes interfaces for IR object dection sensors, a TV remote control along with an LED output and DC motor control (using a 293D H-Bridge driver chip).
When I debugged the application, I used \code\wallhug\wallhug.wat to monitor the operation of the robt code, which follows:
#include// 02.04.28 - wallhug: Created from "Combine" originally // // A 38 kHz IR Signal to be used as an object detection system // // Take Input from Sony IR Remote Control and Display on LCD. // // IR Remote Control Data Stream: // // Leader/Header High "1" "0" // -------+ +----+ +----+ +------------ // | | | | | | // +-----------------+ +-----+ +------------+ // // | 2.4 msecs |540u|660us|540u| 1.2 msecs | // // |1200 msecs| 1.76 msecs | // // | 300 | 440 | // // // Hardware Notes: // PIC16F627 Running at 4 MHz with an external ceramic resonator // RA0 - IR Detector Input, Poll for Collisions // RA4 - LED // RB0 - IR Detector Input, Sony IR Input, Use Falling Interrupt // RB1 - Left Motor, Negative Connection // RB2 - Left Motor, Positive COnnection // RB3 - IR Light Source Output // RB4 - Left ADC // RB5 - Right ADC // RB6 - Right Motor, Negtive Connection // RB7 - Right Motor, Positive Connection // // Global Variables int RTC = 0; // Real Time Clock Counter #define remote0 0x06EF // IR Definitions #define remote1 0x0FEF #define remote2 0x07EF #define remote3 0x0BEF #define remote4 0x03EF #define remote5 0x0DEF #define remote6 0x05EF #define remote7 0x09EF #define remote8 0x01EF #define remote9 0x0EEF #define remoteVolUp 0x0B6F #define remoteVolDown 0x036F #define remoteChanUp 0x0F6F #define remoteChanDown 0x076F #define remotePrevChan 0x022F #define remoteMute 0x0D6F #define remotePower 0x056F unsigned int DataIn; // Data Input unsigned int DataTime = 0; unsigned char DataInCount = 0; // Number of Characters to Read In unsigned int CurrentRTC; volatile char LeftCollision = 0; // Current Collision volatile char RightCollision = 0; volatile char ADCState = 0; // Current ADC State Machine Operating State volatile char ADCDlay = 1; // Delay 50 msecs between each ADC Operation volatile unsigned char LeftADC = 0x0FF; // Last Left ADC Value volatile unsigned char RightADC = 0x0FF; // Last Right ADC Value #define goStop 0x030 // Stop the Motors #define goForward 0x072 // Go Forward #define goReverse 0x0B4 // Reverse the Motors #define turnLeft 0x074 // Left Motor Reversed #define turnRight 0x0B2 // Right Motor Reversed // NOTE: RB4/RB5 Kept High #define LeftForward 1 // Copy "Servo" format #define LeftStop 0 #define LeftReverse -1 #define RightForward 1 #define RightStop 0 #define RightReverse -1 unsigned char motorState = goStop; // Motor State unsigned char PWMDuty = 15; unsigned char PWMCycle = 0; // Start at 1/4 Throttle unsigned char OpnCount = 1; // Want to Monitor the ADC char ExecuteFlag = 0; // Flag Indicating Robot Can Execute // Configuration Fuses #if defined(_16F627) #warning PIC16F627 with external XT oscillator selected __CONFIG(0x03F21); // PIC116F627 Configuration Fuses: // - External "XT" Oscillator // - RA6/RA7 Digital I/O // - External Reset // - 70 msecs Power Up Timer On // - Watchdog Timer Off // - Code Protection Off // - BODEN disabled #else #error Unsupported PICmicro MCU selected #endif // Subroutines // Interrupt Handler void interrupt tmr0_int(void) // TMR0 Interrupt Handler { char temp; if (T0IF) { T0IF = 0; // Reset Interrupt Flag RTC++; // Increment the Clock // Put additional interface code for 1 msec interrupt here // Get Motor State and Turn Off Motors to Start PORTB = temp = (PORTB & 0x039) | 0x030; // Get Current Motor State // Check for Packet which was lost and stop waiting unless packet is active if ((!RB0) && (DataInCount == 0)) { // Data Packet Coming in? DataInCount = 13; // 12 Bits to Come in/13 Edges INTF = 0; // Enable Pin Change Interrupt INTE = 1; } else if (DataInCount == 0) { // Do Collision Detection TRISB3 = 0; // Enable PWM Output while (TMR0 < 64); // For a Limited Number of Cycles if (!RB0) // If Low, then Collision if (LeftCollision < 3) // Must Have 3 Collisions in a Row LeftCollision++; else; else LeftCollision = 0; if (!RA0) // If Low, then Collision if(RightCollision < 3) // Must Have 3 Collisions in a Row LeftCollision++; else; else RightCollision = 0; TRISB3 = 1; } // endif // Motor PWM Code if (PWMDuty == 0) // Always Off ; else if (PWMDuty == 29) // Always On PORTB = temp | motorState; else // Some PWM Value if (PWMCycle <= PWMDuty) PORTB = temp | motorState; else; if (++PWMCycle == 30) // Roll around cycle count? PWMCycle = 0; // Yes // Check Count Operation to Stop Bit if ((!ExecuteFlag) && (--OpnCount == 0)) { // Check the Light Meter OpnCount = 1; // Reset for Next Time Through PORTB = (PORTB & 0x039) | (motorState = goStop); if (LeftADC < RightADC) RA4 = 0; else RA4 = 1; } // endif // Process Remote Control Bit (If Present) if (DataTime) { if (DataInCount == 0) { // New Message Coming In DataInCount = 12; // 12 Bits to Come in DataIn = 0; } else { // Bit to Process DataIn = DataIn << 1; if ((DataTime > 225) && (DataTime < 375)) { // "1" Received DataIn++; if (--DataInCount == 0) { OpnCount = 200; // Timer to Finish if (DataIn == remote2) { motorState = goForward; // Go Forward } else if (DataIn == remote4) { motorState = turnLeft; } else if (DataIn == remote6) { motorState = turnRight; } else if (DataIn == remote8) { motorState = goReverse; } else if (DataIn == remoteVolUp) { if (PWMDuty < 29) PWMDuty++; else; } else if (DataIn == remoteVolDown) { if (PWMDuty != 0) PWMDuty--; else; } else if (DataIn == remotePower) { ExecuteFlag = 1; motorState = goStop; OpnCount = 1; // Timer to Finish } else { // Stop or Nothing ExecuteFlag = 0; motorState = goStop; OpnCount = 1; // Timer to Finish } // endif } // endif } else if ((DataTime > 375) && (DataTime < 500)) { // "0" Received if (--DataInCount == 0) { OpnCount = 200; // Timer to Finish if (DataIn == remote2) { motorState = goForward; // Go Forward } else if (DataIn == remote4) { motorState = turnLeft; } else if (DataIn == remote6) { motorState = turnRight; } else if (DataIn == remote8) { motorState = goReverse; } else if (DataIn == remoteVolUp) { if (PWMDuty < 29) PWMDuty++; else; } else if (DataIn == remoteVolDown) { if (PWMDuty != 0) PWMDuty--; else; } else if (DataIn == remotePower) { ExecuteFlag = 1; motorState = goStop; OpnCount = 1; // Timer to Finish } else { // Stop or Nothing ExecuteFlag = 0; motorState = goStop; OpnCount = 1; // Timer to Finish } // endif } // endif } else if (--DataInCount != 12) // Invalid Timing DataInCount = 0; } // endif DataTime = 0; // Start Over... } // endif // Add to the Bit Delay Time CurrentRTC += 0x0100; // Keep Upcoming // Check To see if there is a missing pulse... if ((DataInCount) && (CurrentRTC > 0x00900)) DataInCount = 0; // Reset and Wait for the Next Character // ADC State Machine: if (!DataInCount) { // Only Execute if NOT Rx'g IR Command if (ADCState == 0) { // ADC Start Delay if (--ADCDlay == 0) ADCState++; } else if (ADCState == 1) { // Start Charging Left and Right (for 1 msec min.) PORTB = PORTB | 0x030; TRISB4 = 0; TRISB5 = 0; ADCState++; // Jump to next state in 1 msec } else if (ADCState == 2) { TRISB4 = 1; // Turn off Left Pin Driver temp = PORTB; // Clear pending Interrupts on Port Change RBIF = 0; RBIE = 1; ADCState++; } else if (ADCState == 3) { // Roll Over, No Interrupt LeftADC = 0x0FF; temp = PORTB; // Clear pending Interrupts on Port Change RBIF = 0; RBIE = 0; ADCState++; } else if (ADCState == 4) { // Start Charging Right (for 1 msec) TRISB5 = 1; // Change TRISB to Input temp = PORTB; // Clear pending Interrupts on Port Change RBIF = 0; RBIE = 1; ADCState++; } else if (ADCState == 5) { RightADC = 0x0FF; temp = PORTB; // Clear pending Interrupts on Port Change RBIF = 0; RBIE = 0; ADCDlay = 50; ADCState = 0; } // endif } // endif } // endif // Put Different Interrupt Handlers here if (INTF) { // RB0/INT Pin Interrupt DataTime = CurrentRTC + TMR0; // Get the Bit Timing CurrentRTC = 0x0FFFF - TMR0; // Get Timing for the Next Bit INTF = 0; // Reset Interrupt } // endif if (RBIF) { // Interrupt on Pin Change if (ADCState == 3) { LeftADC = TMR0; // Get the ADC Count ADCState++; } else { // Step 5 (Right ADC) RightADC = TMR0; // Get the ADC Count ADCDlay = 50; // Repeat Every 50 msecs ADCState = 0; } // endif temp = PORTB; // Clear pending Interrupts on Port Change RBIF = 0; RBIE = 0; } // endif } // End Interrupt Handler // User Subroutines void Dlay(int msecs) // Delay the Set Number of msecs { int valueDlay; valueDlay = RTC + msecs + 1; // Get the Final Delay Value while (valueDlay != RTC); // Wait for it } // End Dlay void LEDOutput(int state ) // Set the LED State { if (state) RA4 = 0; // LED On else RA4 = 1; // LED Off } // End LEDOutput int GetLeftLight() { return LeftADC; } // End GetLeftLight int GetRightLight() { return RightADC; } // End GetRightLight int GetLeftWhisker() // Return State of Left Whisker { if (LeftCollision == 3) return 1; // Somthing Pressing on Whisker else return 0; } // End GetLeftWhisker int GetRightWhisker() // Return State of Right Whisker { if (RightCollision == 3) return 1; // Somthing Pressing on Whisker else return 0; } // End GetRightWhisker void LeftMotor(int Movement, int Speed) { switch (Movement) { // Set Movement case 1: // Forward motorState = (motorState & 0x0F9) + 2; break; case 0: // Stop motorState = motorState & 0x0F9; break; case -1: // Reverse motorState = (motorState & 0x0F9) + 4; break; } // endif PWMDuty = Speed; } // End LeftMotor void RightMotor(int Movement, int Speed) { switch (Movement) { // Set Movement case 1: // Forward motorState = (motorState & 0x03F) + 0x040; break; case 0: // Stop motorState = motorState & 0x03F; break; case -1: // Reverse motorState = (motorState & 0x03F) + 0x080; break; } // endif PWMDuty = Speed; } // End rightMotor // Mainline void main(void) // Template Mainline { OPTION = 0x0D1; // Assign Prescaler to TMR0 // Prescaler is /4 TMR0 = 0; // Reset the Timer for Start T0IE = 1; // Enable Timer Interrupts GIE = 1; // Enable Interrupts // Put in Interface initialization code here INTEDG = 1; // Interrupt on Rising Edge of RB0/INT IR TX'r CCPR1L = 13; // 50% Duty Cycle CCP1CON = 0b000001111; // Turn on PWM Mode PR2 = 26; // 26 usec Cycle for 38 KHz TMR2 = 0; // Reset TMR2 T2CON = 0b000000100; // TMR2 is On, Scalers 1:1 CMCON = 0x007; // PORTA Analog Functions Off PORTA = 0x010; // RA4 is the LED TRISA = 0x0EF; PORTB = 0x000; TRISB = 0x039; // RB7/RB6 and RB2/RB1 Are Motor Drivers while (1 == 1) { // Loop forever if (ExecuteFlag) { // Put in Robot Biologic Code Here } else { // Put in Biologic Code Reset Statements Here } // endif } // endwhile } // End of Mainline
The second prototype robot platform that I came up with was built on a small piece of 3/8" plywood and used two modified servos for driving the robot. The servos were modified by converting them from moving only 90 degrees to turning continuously. The procedures for modifying servos is different for different manufacturer's products and part numbers. I did find instructions for modifying different servos on the Internet - you should make sure that before you buy servos, that you have instructions showing you how to modify them for continuous motion. The servos that I used for the platform used with this book cost me less than $15 each (and these servos had ball bearing outputs for longer part life).
Remember that when you are modifying the servos, to use a multi-turn, "trimmer" potentiometer, mounted in such a way that the "center" (no movement) setting can be made easily. Some instructions for modifying the servos will suggest that you just use two resistors of the same value, but this will mean you will have to "tune" your software to match the resistor values. It is much easier to add a potentiometer to the servo and trim it to match the controller's "stop timing".
The \code\servo\servo2.c code is based on the \code\wallhug\wallhug.c code. When I was simulating the applcation, I came use the \code\servo\servo.wat file. The base code for the application is:
#include// 02.04.28 - Updated by Myke Predko by Processing Remote // Control Bits in TMR0 Interrupt Handler // 02.04.26 - Updated with IR Remote Control // 02.02.23 - Myke Predko // // Servo Test Application. Run the servos Forwards, Backwards, // Together and Apart // // Setup TMR0 to interrupt Mainline once every 512 usecs // // // Hardware Notes: // PIC16F627 Running at 4 MHz with an External Oscillator // RA1 - Comparator1, Tied to Vss // RA2 - 1/2 Vdd from Vref module, output to check Vref operation // RB0 - IR Detector for Remote Control static volatile bit IRDetect @ (unsigned)&PORTB*8+0; // RB1 - Right Whisker static volatile bit RightWhiskerPin @ (unsigned)&PORTB*8+1; // RB2 - Left Whisker static volatile bit LeftWhiskerPin @ (unsigned)&PORTB*8+2; // RB3 - Output LED static volatile bit LED @ (unsigned)&PORTB*8+3; static volatile bit LEDTRIS @ (unsigned)&TRISB*8+3; // RB4 - Left Servo static volatile bit leftServo @ (unsigned)&PORTB*8+4; static volatile bit leftServoTRIS @ (unsigned)&TRISB*8+4; // RB5 - Right Servo static volatile bit rightServo @ (unsigned)&PORTB*8+5; static volatile bit rightServoTRIS @ (unsigned)&TRISB*8+5; // Global Variables unsigned int RTC = 0; // Real Time Clock Counter unsigned char RTCDlay = 2; // With 512 Cycle Clock, Increment 1/2x // Servo Variables #define LeftForward 38 // Left Motor Values #define LeftStop 37 #define LeftReverse 36 #define RightForward 36 // Right Motor Values #define RightStop 37 #define RightReverse 38 char ServoDlay = 1; // Output Servo Every 20 msecs char leftSpeed = LeftStop; char rightSpeed = RightStop; // Remote Variables #define remote0 0x06EF // IR Definitions #define remote1 0x0FEF #define remote2 0x07EF #define remote3 0x0BEF #define remote4 0x03EF #define remote5 0x0DEF #define remote6 0x05EF #define remote7 0x09EF #define remote8 0x01EF #define remote9 0x0EEF #define remoteVolUp 0x0B6F #define remoteVolDown 0x036F #define remoteChanUp 0x0F6F #define remoteChanDown 0x076F #define remotePrevChan 0x022F #define remoteMute 0x0D6F #define remotePower 0x056F unsigned int DataIn; // Data Input unsigned char DataInCount = 0; // Number of Characters to Read In unsigned int DataTime = 0; unsigned int CurrentRTC = 0; int OpnCount = 1; // If Stopped, Check Comparator volatile char LeftWhisker = 0; // Count of the Left Whisker volatile char RightWhisker = 0; // Count of the Right Whisker char ExecuteFlag = 0; // Flag Indicating Robot Can Execute // Configuration Fuses #if defined(_16F627) #warning PIC16F627 with external XT oscillator selected __CONFIG(0x03F21); // PIC116F627 Configuration Fuses: // - External "XT" Oscillator // - RA6/RA7 Digital I/O // - External Reset // - 70 msecs Power Up Timer On // - Watchdog Timer Off // - Code Protection Off // - BODEN Disabled #else #error Unsupported PICmicro MCU selected #endif // Interrupt Handler void interrupt tmr0_int(void) // TMR0 Interrupt Handler { if (T0IF) { T0IF = 0; // Reset Interrupt Flag if (--ServoDlay == 0) { // New 20 msec Delay ServoDlay = 40; // Reset 20 msec Dlay leftServo = 1; // Make Servos High rightServo = 1; } else { // Output the Pulses if (leftSpeed >= ServoDlay) leftServo = 0; // Servo Pulse Stop if (rightSpeed >= ServoDlay) rightServo = 0; // Servo Pulse Stop } // endif // Process Remote Control Bit (If Present) if (DataTime) { if (DataInCount == 0) { // New Message Coming In DataInCount = 12; // 12 Bits to Come in DataIn = 0; } else { // Bit to Process DataIn = DataIn << 1; if ((DataTime > 450) && (DataTime < 750)) { // "1" Received DataIn++; if (--DataInCount == 0) { OpnCount = 400; // Timer to Finish leftSpeed = LeftForward; rightSpeed = RightForward; if (DataIn == remote2) ; // Go Forward else if (DataIn == remote4) leftSpeed = LeftReverse; else if (DataIn == remote6) rightSpeed = RightReverse; else if (DataIn == remote8) { leftSpeed = LeftReverse; rightSpeed = RightReverse; } else if (DataIn == remotePower) { ExecuteFlag = 1; leftSpeed = LeftStop; rightSpeed = RightStop; OpnCount = 1; // Timer to Finish } else { // Stop or Nothing ExecuteFlag = 0; leftSpeed = LeftStop; rightSpeed = RightStop; OpnCount = 1; // Timer to Finish } // endif } // Endif } else if ((DataTime > 750) && (DataTime < 1000)) { // "0" Received if (--DataInCount == 0) { OpnCount = 400; // Timer to Finish leftSpeed = LeftForward; rightSpeed = RightForward; if (DataIn == remote2) ; // Go Forward else if (DataIn == remote4) leftSpeed = LeftReverse; else if (DataIn == remote6) rightSpeed = RightReverse; else if (DataIn == remote8) { leftSpeed = LeftReverse; rightSpeed = RightReverse; } else if (DataIn == remotePower) { ExecuteFlag = 1; leftSpeed = LeftStop; rightSpeed = RightStop; OpnCount = 1; // Timer to Finish } else { // Stop or Nothing ExecuteFlag = 0; leftSpeed = LeftStop; rightSpeed = RightStop; OpnCount = 1; // Timer to Finish } // endif } // Endif } else // Invalid - Delete DataInCount = 0; } // endif DataTime = 0; // Start Over... } // endif // Add to the Bit Delay Time CurrentRTC += 0x0100; // Keep Upcoming // Check To see if there is a missing pulse... if ((DataInCount) && (CurrentRTC > 0x01200)) DataInCount = 0; // Reset and Wait for the Next Character // 1 msec Interrupt Code if (--RTCDlay == 0) { RTCDlay = 2; // Reset the Clock RTC++; // Increment the Clock // Increment Counter if Left Whisker Held Down if (!LeftWhiskerPin) // Left Whisker Pressed if (LeftWhisker < 20) // debounced LeftWhisker++; else; // Whisker Pressed for 20 msecs else // Whisker Released if (LeftWhisker != 0) // Whisker Release being debounced LeftWhisker--; else; // Whisker released for more than 20 msecs // Increment Counter if Right Whisker Held Down if (!RightWhiskerPin) // Right Whisker Pressed if (RightWhisker < 20) // debounced RightWhisker++; else; // Whisker Pressed for 20 msecs else // Whisker Released if (RightWhisker != 0) // Whisker Release being debounced RightWhisker--; else; // Whisker released for more than 20 msecs // Check Count Operation to Stop Bit if ((!ExecuteFlag) && (--OpnCount == 0)) { // Check the Light Meter OpnCount = 1; // Reset for Next Time Through leftSpeed = LeftStop; rightSpeed = RightStop; if (C2OUT) LED = 0; else LED = 1; } // endif } // endif } // endif // Put Different Interrupt Handlers here if (INTF) { // RB0/INT Pin Interrupt DataTime = CurrentRTC + TMR0; // Get the Bit Timing CurrentRTC = 0x0FFFF - TMR0; // Get Timing for the Next Bit INTF = 0; // Reset Interrupt } // endif } // End Interrupt Handler // User Subroutines void Dlay(int msecs) // Delay the Set Number of msecs { int valueDlay; valueDlay = RTC + msecs + 1; // Get the Final Delay Value while (valueDlay != RTC); // Wait for it } // End Dlay void LEDOutput(int state ) // Set the LED State { if (state) LED = 0; // LED On else LED = 1; // LED Off } // End LEDOutput int GetLeftLight() { if (C2OUT) return 0x080; // Indicate Left is Brigher else return 0x0FF; } // End GetLeftLight int GetRightLight() { if (!C2OUT) return 0x080; // Indicate Right is Brigher else return 0x0FF; } // End GetRightLight int GetLeftWhisker() // Return State of Left Whisker { if (LeftWhisker == 20) return 1; // Somthing Pressing on Whisker else return 0; } // End GetLeftWhisker int GetRightWhisker() // Return State of Right Whisker { if (RightWhisker == 20) return 1; // Somthing Pressing on Whisker else return 0; } // End GetRightWhisker void LeftMotor(int Movement) { leftSpeed = Movement; // Use "leftForward", etc. for motions } // End LeftMotor void RightMotor(int Movement) { rightSpeed = Movement; // Use "rightForward", etc. for motions } // End rightMotor // Mainline void main(void) // Template Mainline { OPTION = 0x0D0; // Assign Prescaler to TMR0 \2 // #### - Note Different from Standard TMR0 = 0; // Reset the Timer for Start T0IE = 1; // Enable Timer Interrupts GIE = 1; // Enable Interrupts // Put in Interface initialization code here INTEDG = 1; // Interrupt on Rising Edge of RB0/IR TX'r INTF = 0; // Make Sure NO Pending Interrupt INTE = 1; // Enable RBO/INT Pin Interrupts CMCON = 0x002; // Enable the Comparator Module // C1/C2 Normal, not inverted // CIS = 0 // Mode 2, compare against Vdd TRISA = 0x007; // Just bits RA0 and RA1 are inputs VRCON = 0x0EC; // Enable Vref module // Vref output on RA2 // High Vref Range // Ladder value of 12 LED = 1; // Enable the LED LEDTRIS = 0; leftServo = rightServo = 0; // Initially Low Output leftServoTRIS = rightServoTRIS = 0; // Biologic Code Control while (1 == 1) { // Loop forever if (ExecuteFlag) { // Put in Robot Biologic Code Here } else { // Put in Biologic Code Reset Statements Here } // endif } // endwhile } // End of Mainline
To demonstrate the operation of a state machine as well as demonstrate how the differenent elelogic and mechalogic functions could be integrated together to create a base for common biologic code, I came up with the very basic biologic application:
// Put in Robot Biologic Code Here
switch (State) { // Process the state command
case 1: // Go forward for 2 Seconds
State++; // Start on State 2
StateDlay = RTC + 2000;
LeftMotor(LeftForward);
RightMotor(RightForward);
break;
case 2: // Wait for 2 Seconds to be up with no Collisions
if (GetLeftWhisker())
State = 10;
else if (GetRightWhisker())
State = 20;
else if (StateDlay == RTC)
State++; // No Collision, Turn for 3/4 Seconds
break;
case 3: // Turn Right for 3/4 Seconds
State++;
StateDlay = RTC + 750;
LeftMotor(LeftForward);
RightMotor(RightReverse);
break;
case 4: // Wait for 3/4 Seconds to be up with no Collisions
if (GetLeftWhisker())
State = 10;
else if (GetRightWhisker())
State = 20;
else if (StateDlay == RTC)
State = 1; // No Collision, Start Over
break;
case 10: // Left Collision, Back away for 1/2 Second
State++;
StateDlay = RTC + 500;
LeftMotor(LeftReverse);
RightMotor(RightReverse);
break;
case 11: // Wait 1/2 Second
if (StateDlay == RTC)
State++;
break;
case 12: // Turn to Right Away from Collision source for 1/2 Second
State++;
StateDlay = RTC + 500;
LeftMotor(LeftForward);
RightMotor(RightReverse);
break;
case 13: // Wait 1/2 Second and resume
if (StateDlay == RTC)
State = 1;
break;
case 20: // Right Collision, BAck away for 1/2 Second
State++;
StateDlay = RTC + 500;
LeftMotor(LeftReverse);
RightMotor(RightReverse);
break;
case 21: // Wait 1/2 Second
if (StateDlay == RTC)
State++;
break;
case 22: // Turn to Left Away from Collision source for 1/2 Second
State++;
StateDlay = RTC + 500;
LeftMotor(LeftReverse);
RightMotor(RightForward);
break;
case 23: // Wait 1/2 Second and resume
if (StateDlay == RTC)
State = 1;
break;
} // endswitch
} else {
// Put in Biologic Code Reset Statements Here
State = 1; // Random Operation State
StateDlay = 0;
This code has been integrated into the \code\statemc\wallhug.c wall hugging mouse based robot as well as the \code\statemc\servo.c R/C servo based robot. Using this code as a guide, you can start looking at your own robot applications.